/**
 * @file GraphSearch.h
 * @author rj.wang (rj.wang@aotbot.com)
 * @brief 路径规划算法 - 图搜
 * @version 0.1
 * @date 2024-11-04
 * @copyright Copyright (c) 2024
 */

#ifndef __GRID_GRAPH_SEARCH_H_
#define __GRID_GRAPH_SEARCH_H_
#include <map>
#include <deque>
#include <algorithm>
#include <cmath>
#include <limits>
#include <sstream>
#include <memory>

#include "DF.h"

namespace common {
namespace planning{

struct Node {
  enum Dir {
    NONE = 0,
    RIGHT,
    RIGHT_UP,
    UP,
    LEFT_UP,
    LEFT,
    LEFT_DOWN,
    DOWN,
    RIGHT_DOWN,
  };

  using Ptr = std::shared_ptr<Node>;

  int64_t x, y;
  uint8_t dir{NONE};
  float g{0};
  float f{0};
  Ptr parent{nullptr};

  Node(int64_t ix = 0, int64_t iy = 0, uint8_t direction = NONE) : 
    x(ix), y(iy), dir(direction) {}
  
  /* explicit  */Node(const Node& node)
     : x(node.x), y(node.y), dir(node.dir), g(node.g), f(node.f), 
       parent(node.parent) {}

  Node& operator = (const Node& node) {
    x = node.x;
    y = node.y;
    dir = node.dir;
    g = node.g;
    f = node.f;
    parent = node.parent;
    return *this;
  }

  bool operator < (const Node& right) const {
    return (this->x < right.x) || 
      (this->x == right.x && this->y < right.y);
  }
};

struct CompareF {
  bool operator() (Node::Ptr left, Node::Ptr right) const { 
    return (left->f < right->f) || 
      (left->f == right->f && left->x < right->x) || 
      (left->f == right->f && left->x == right->x && left->y < right->y); 
  }
};

struct CompareXY {
  bool operator() (Node::Ptr left, Node::Ptr right) const { 
    return (left->x < right->x) || 
      (left->x == right->x && left->y < right->y); 
  }
};

struct GridXY {
  enum {
    UNEXPANDED = 0,
    OPEN = 1,
    CLOSED = 2,
  };

  uint8_t status{UNEXPANDED};
  Node::Ptr np{nullptr};

  GridXY() {}

  GridXY(const uint8_t gstatus, const Node::Ptr& gnp) : status(gstatus), np(gnp) {}

  GridXY& operator = (const GridXY& grid) {
    status = grid.status;
    np = grid.np;
    return *this;
  }
};

using FNodePtrs = std::set<Node::Ptr, CompareF>;
using XYNodePtrs = std::set<Node::Ptr, CompareXY>;
using Nodes = std::deque<Node>;
using PolyNodes = std::deque<Node>;
using NodeSet = std::set<Node>;

class GraphSearch {
public:
  enum Type {
    DIJKSTRA = 0,
    ASTAR = 1,
    JPS = 2,
  };

  GraphSearch() {}
  ~GraphSearch() {}

  bool Plan(
      const Node& start, 
      const Node& goal, 
      const float obs_thresh, 
      const float free_thresh,
      const uint8_t type = ASTAR);

  Nodes GetOptimizedPath();

  inline void SetStart(const Node& node) {
    start_ = std::make_shared<Node>(node.x, node.y, Node::Dir::NONE);
  }

  inline void SetGoal(const Node& node) {
    goal_ = std::make_shared<Node>(node.x, node.y, Node::Dir::NONE);
  }

  const Nodes& GetPath();

  inline const Nodes& Path() { return path_; }

  inline void SetPath(const Nodes& path) { path_ = path;}

  inline void SetShortestPath(const Nodes& path) { 
    shortest_path_ = path; 
  }

  inline size_t GetVisitedSize() const {
    return open_list_f_.size() + closed_list_size_;
  }

  inline void Map(const DistanceMap& dmap) { map_ = dmap; }

  inline DistanceMap& DFMap() { return map_; }

  inline bool Visited(const Node::Ptr node) { 
    return expanded_.at(node->x + map_.Width() * node->y).status != GridXY::UNEXPANDED;
  }

  void PubPlanResult();

  void PrintPlanResult() const;

  void PrintF(FNodePtrs& fnodes) const {
    std::cout << "nodes are:" << std::endl;
    for(const auto& node : fnodes) {
      std::cout << "(" << node->x << ", " << node->y << ")" << std::endl;
    }
  }

  void PrintXY(XYNodePtrs& xnodes) const {
    std::cout << "nodes are:" << std::endl;
    for(const auto& node : xnodes) {
      std::cout << "(" << node->x << ", " << node->y << ") ";
      std::cout << "parent (" << node->parent->x << ", " << node->parent->y << ")" << std::endl;
    }
  }

private:
  void InitSearch();

  void PopOpen(Node::Ptr& np);

  void ProcessNeighbours(const Node::Ptr& np);

  bool Plan();

  inline bool IsSameNode(Node::Ptr a, Node::Ptr b) const {
    return a->x == b->x && a->y == b->y;
  }

  void ProcessNeighboursDijkstra(const Node::Ptr& np);

  void ProcessNeighboursAstar(const Node::Ptr& np);

  void ProcessNeighboursJps(const Node::Ptr& np);

  Nodes PruneNeighbours(const Node::Ptr& np) const;

  Nodes PruneNeighboursNone(const Node::Ptr& np) const;

  Nodes PruneNeighboursRight(const Node::Ptr& np) const;

  Nodes PruneNeighboursRightUp(const Node::Ptr& np) const;

  Nodes PruneNeighboursUp(const Node::Ptr& np) const;

  Nodes PruneNeighboursLeftUp(const Node::Ptr& np) const;

  Nodes PruneNeighboursLeft(const Node::Ptr& np) const;

  Nodes PruneNeighboursLeftDown(const Node::Ptr& np) const;

  Nodes PruneNeighboursDown(const Node::Ptr& np) const;

  Nodes PruneNeighboursRightDown(const Node::Ptr& np) const;

  Node::Ptr Jump(const Node& node) const;

  Node Step(const Node& node) const;

  bool Forced(const Node::Ptr np) const;

  Node::Ptr start_{nullptr};
  Node::Ptr goal_{nullptr};
  FNodePtrs open_list_f_; //open_list
  std::vector<GridXY> expanded_; //expanded grids
  uint64_t closed_list_size_{0}; //size of closed_list
  
  Nodes path_;
  Nodes shortest_path_;
  DistanceMap map_; //store squared distance to closest obstacle
  uint8_t search_type_{Type::ASTAR};
};

//for graph search
inline float Int2Float(const int64_t ix) { 
  return ix + 0.5f;
}

inline int64_t Float2Int(const float x) {
  return static_cast<int64_t>(floor(x));
}

//for path optimize
Nodes OptimizePath(
    const DistanceMap& map, 
    const Nodes& path, 
    const float thresh_distance);

bool IsPathFree(
    const DistanceMap& map, 
    const Node& start, 
    const Node& end, 
    const float thresh_distance);

Nodes Critical2Path(
    const Nodes& critical_path, 
    const float resolution);

inline float CalLineDir(
    const int64_t x_start, 
    const int64_t y_start, 
    const int64_t x_end, 
    const int64_t y_end) {
  return atan2(static_cast<float>(y_end) - static_cast<float>(y_start), 
    static_cast<float>(x_end) - static_cast<float>(x_start));
}

Nodes Bresenham(const Node& start, const Node& end);

} //end of namespace planning
} //end of namespace common
#endif